//
// Created by bit on 2022/8/1.
//

#include "guidance_filter/PosFilter.h"

PosKalmanFilter::PosKalmanFilter(float dt) {
    dt_ = dt;
    init();
    time_start_ = currentTimeMs();
//    output_data.open(log_path.c_str());
//    output_data << "timestamp,N_input,E_input,N,E,Vn,Ve,V_total,heading,dt_"<< std::endl;
}

void PosKalmanFilter::init() {

    A_m_ << 1, 0, dt_, 0, 0, 1, 0, dt_, 0, 0, 1, 0, 0, 0, 0, 1;
    B_m_ << 0.5f*pow(dt_, 2), 0.5f * pow(dt_, 2), dt_, dt_;
    H_m_ << 1, 0, 0, 0, 0, 1, 0 ,0;
    S_m_.setIdentity();
    S_m_ *= state_uncertainty_;
    Q_m_ << 0.25*pow(dt_, 2), 0, 0.5 * pow(dt_, 3), 0,
            0, 0.25*pow(dt_, 2), 0, 0.5 * pow(dt_, 3),
            0.5*pow(dt_, 3), 0, pow(dt_, 2), 0,
            0, 0.5*pow(dt_, 3), 0, pow(dt_, 2);
    R_m_.setIdentity();
    R_m_ *= Means_uncertainty_;
    eye4_.setIdentity();
    x_m_ << 0, 0, 0, 0;
}

bool PosKalmanFilter::update(float N_input, float E_input, float &N, float &E, float &v_N, float &v_E) {
    bool ret{false};
    float dt{0.1};
    // TODO: 长时间不更新后，需要重新初始化

    switch (init_phase_) {
        case 0: {
            N_init_ = N_input;
            E_init_ = E_input;
            time_init_ = currentTimeMs();
            init_phase_ += 1;
//            output_data << mycurrentTime() - time_start_ << "," << N_input << "," << E_input << std::endl;
            break;
        }
        case 1:
        case 2:
        case 3:
//            output_data << mycurrentTime() - time_start_ << "," << N_input << "," << E_input << std::endl;
            init_phase_ += 1;
            break;
        case 4: {
            float init_dt = elapsedTimeMs(time_init_) / 1000.0f;
            float v_n_tmp = (N_input-N_init_)/ init_dt;
            float v_e_tmp = (E_input-E_init_)/ init_dt;

//            float v_total = pow((pow(v_n_tmp,2) + pow(v_e_tmp,2)), 0.5f);
//            float heading = atan2(v_e_tmp, v_n_tmp) * 57.3f;

//            output_data << mycurrentTime() - time_start_ << "," << N_input << "," << E_input
//                        << ",,," << v_n_tmp << "," << v_e_tmp << ","
//                        << v_total << "," << heading << std::endl;

            N_input -= N_init_;
            E_input -= E_init_;
            E_input = angle_lim(E_input, 180);
            setInitState(N_input, E_input, v_n_tmp, v_e_tmp);
            init_phase_ += 1;
            break;
        }
        default: {
            ret = true;
            N_input -= N_init_;
            E_input -= E_init_;
            E_input = angle_lim(E_input, 180);
            if (time_loop_) {
                dt = elapsedTimeMs(time_loop_) / 1000.0f;
            }
            time_loop_ = currentTimeMs();

            if (abs(dt - dt_) > 0.01) {
                // 判断dt是否需要更新
//                std::cout << "update time " << std::endl;
                dt_ = dt;
                A_m_ << 1, 0, dt_, 0, 0, 1, 0, dt_, 0, 0, 1, 0, 0, 0, 0, 1;
                B_m_ << 0.5f*pow(dt_, 2), 0.5f * pow(dt_, 2), dt_, dt_;
                Q_m_ << 0.25*pow(dt_, 2), 0, 0.5 * pow(dt_, 3), 0,
                        0, 0.25*pow(dt_, 2), 0, 0.5 * pow(dt_, 3),
                        0.5*pow(dt_, 3), 0, pow(dt_, 2), 0,
                        0, 0.5*pow(dt_, 3), 0, pow(dt_, 2);
            }

            input_(0,0) = N_input;
            input_(1,0) = E_input;

            x_m_ = A_m_ * x_m_ + B_m_ * u_input_;
            S_m_ = A_m_ * S_m_ * A_m_.transpose() + Q_m_*0.1;

            K_m_ = (S_m_ * H_m_.transpose()) * ((H_m_ *S_m_ *H_m_.transpose() + R_m_).inverse());
            x_m_ = x_m_ + K_m_ * (input_ - H_m_ * x_m_);
            S_m_ = (eye4_ - K_m_ * H_m_) * S_m_;

            N = x_m_(0,0) + N_init_;
            E = x_m_(1,0) + E_init_;
            v_N = x_m_(2,0);
            v_E = x_m_(3,0);
            E = angle_lim(E, 180);

//            float v_total = pow((pow(v_N,2) + pow(v_E,2)), 0.5f);
//            float heading = atan2(v_E, v_N) * 57.3f;

//            output_data << mycurrentTime() - time_start_ << "," << N_input << "," << E_input
//                        << "," << N << "," << E
//                        << "," << v_N << "," << v_E
//                        << "," << v_total << "," << heading << "," << dt_ << std::endl;
        }
    }
    return ret;
}

void PosKalmanFilter::setInitState(float N, float E, float v_n, float v_e) {
    std::cout << "set init state N " << N << ", E " << E << ", v_n " << v_n << ", v_e " << v_e << std::endl;
    x_m_ << N, E, v_n, v_e;
}

void PosKalmanFilter::setMeansUncertainty(float value) {
    if (abs(Means_uncertainty_ - value) > 1e-6) {
        Means_uncertainty_ = value;
        R_m_.setIdentity();
        R_m_ *= Means_uncertainty_;
        std::cout << "[kalman_]: set Means_uncertainty_=" << value << std::endl;
    }
}
